/*
 * Conditioner.cpp
 *
 *  Created on: Dec 11, 2011
 *      Author: Michael Anderson
 *      Part of Team 955 Battery Cart project
 *      compiled on Eclipse with Arduino libraries and avr-gcc
 */

#include "Conditioner.h"

Conditioner::Conditioner(uint8_t ID, EventManager &log) {
    if (ID >= 0 && ID < NUM_CONDITIONERS) { //Make sure we're within range
        conditionerID = ID;
        eManager = &log;
    }
    else {
        //Utils::showError((char*)"CondID out of range");
    }
    wattHours = 0.;
    cutoffVoltageCheckTimer = millis();
    state = CONDITIONER_IDLE;
}

void Conditioner::run() {
    if (state > CONDITIONER_CONNECTED) {
        if (state == CONDITIONER_TEST_QUICK) {
            uint32_t curTime = millis();
            if (getSecondsSince(conditionTimer, curTime) < PULSE_TEST_LENGTH_SECONDS) {
                if (getRawVoltage() < BATTERY_TEST_RAW_VOLTAGE_THRESHOLD) {
                    quickTestResult = QUICK_TEST_STATUS_NO_PASS;
                    disconnect(false);
                }
                else if (getRawCurrent() < BATTERY_TEST_RAW_CURRENT_THRESHOLD) {
                    quickTestResult = QUICK_TEST_STATUS_ERROR;
                    disconnect(false);
                }
            }
            else {
                cutoffVoltageCheckTimer = curTime;
                if (getRawVoltage() > BATTERY_TEST_RAW_VOLTAGE_THRESHOLD) {
                    if (getRawCurrent() > BATTERY_TEST_RAW_CURRENT_THRESHOLD)
                        quickTestResult = QUICK_TEST_STATUS_PASS;
                    else
                        quickTestResult = QUICK_TEST_STATUS_ERROR;
                }
                else
                    quickTestResult = QUICK_TEST_STATUS_NO_PASS;
                disconnect(false);
            }
        }
        else if (state == CONDITIONER_TEST_HIGH_RATE) {
            if (waitForTest)  {
                if (getMillisSince(testWaitStartTime, millis()) > TEST_WAIT_MILLIS) {
                    waitForTest = false;
                    testWaitStartTime = millis();
                    setStage(TEST_LOAD_CONFIG);
                }
            }
            if (!waitForTest) {
                if (getRawVoltage() < BATTERY_TEST_RAW_VOLTAGE_THRESHOLD) {
                    highRateTestStartTime = getMillisSince(highRateTestStartTime, millis());
                    highRateTestResult = (double)(highRateTestStartTime / MILLIS_PER_MINUTE);
                    highRateTestResult *= BATTERY_TEST_MINUTES_TO_WATT_HOURS;
                    disconnect(false);
                }
            }
        }
        else {
            //if a load is applied to the conditioner
            uint32_t curTime = millis();
            if (getMillisSince(cutoffVoltageCheckTimer, curTime) >= INVERTER_CHECK_DELAY && !inverterIsActive()) {
                //if enough time has passed and the battery is done discharging on the current load
                if (ENABLE_SERIAL_DEBUG)
                    Serial.println("inverter not active");
                if (stage > CONDITIONER_LOAD_CONFIG_20W) {
                    conditioner_state oldState = state;
                    setStage(load_config(stage - 1));
                    cutoffVoltageCheckTimer = curTime;
                    if (ENABLE_SERIAL_DEBUG)
                        Serial.println("Swapping stage!");
                    eManager->conditionerEvent(conditionerID, connectedBattery->getID(), EVENT_CONDITIONER_SWAP_STAGE, oldState, state, 0l);
                }
                else {
                    if (ENABLE_SERIAL_DEBUG)
                        Serial.println("Conditioning complete");
                    cutoffVoltageCheckTimer = curTime;
                    disconnect(false);
                }
            }
            addWattHours(getMillisSince(powerReadTimer, curTime), getVoltage(), getCurrent());
            powerReadTimer = curTime;
        }
    }
}

//returns true if the inverter is able to continue discharging
bool Conditioner::inverterIsActive() {
    return (getRawVoltage() > COND_MIN_RAW_V && getRawCurrent() > COND_MIN_RAW_I);
}

battery_quick_test_status Conditioner::getQuickTestResult() {
    return quickTestResult;
}

double Conditioner::getHighRateTestResult() {
    return highRateTestResult;
}

//connects a battery to the conditioner if the conditioner and battery is idle
bool Conditioner::connect(Battery &batt) {
    if (state == CONDITIONER_IDLE && batt.getState() == BATTERY_IDLE) {
        //TODO: Add timer code
        state = CONDITIONER_CONNECTED;
        connectedBattery = &batt;
        batt.setState(BATTERY_CONNECTED_TO_CONDITIONER);
        eManager->conditionerEvent(conditionerID, connectedBattery->getID(), EVENT_CONDITIONER_CONNECTED, CONDITIONER_IDLE, CONDITIONER_CONNECTED, 0l);
        return true;
    }
    return false;
}

//disconnects the battery from the conditioner and stops conditioning
bool Conditioner::disconnect(bool halting) {
    if (state != CONDITIONER_IDLE) {
        if (ENABLE_SERIAL_DEBUG) {
            char str[32];
            char str2[32];
            sprintf(str, "Cond %i, V=%i, A=%i", conditionerID, (uint16_t)getVoltage(), (uint16_t)getCurrent());
            sprintf(str2, "A0=%i, A1=%i, A2=%i, A3=%i", Utils::safeAnalogRead(A0), Utils::safeAnalogRead(A1), Utils::safeAnalogRead(A2), Utils::safeAnalogRead(A3));
            Serial.println(str);
            Serial.println(str2);
        }
        conditioner_state oldState = state;
        stop(halting);
        eManager->conditionerEvent(conditionerID, connectedBattery->getID(), EVENT_CONDITIONER_DISCONNECTED, oldState, state, 0l);
        return true;
    }
    return false;
}

//starts the conditioning process if the conditioner is connected
bool Conditioner::start(discharge_type type, bool waitForTest) {
    if (state == CONDITIONER_CONNECTED) {
        switch(type) {
            case DISCHARGE_CONDITION_PULSE: {
                connectedBattery->setState(BATTERY_CONDITIONING);
                action = type;
                setStage(CONDITIONER_LOAD_CONFIG_300W);
                cutoffVoltageCheckTimer = millis();
                wattHours = 0.;
                powerReadTimer = millis();
                eManager->conditionerEvent(conditionerID, connectedBattery->getID(), EVENT_CONDITIONER_START, CONDITIONER_CONNECTED, CONDITIONER_CONDITION_300W, 0l);
                return true;
                break;
            }
            case DISCHARGE_CONDITION_LOW: {
                connectedBattery->setState(BATTERY_CONDITIONING);
                action = type;
                setStage(CONDITIONER_LOAD_CONFIG_20W);
                cutoffVoltageCheckTimer = millis();
                wattHours = 0.;
                powerReadTimer = millis();
                eManager->conditionerEvent(conditionerID, connectedBattery->getID(), EVENT_CONDITIONER_START, CONDITIONER_CONNECTED, CONDITIONER_CONDITION_300W, 0l);
                return true;
                break;
            }
            case DISCHARGE_TEST_QUICK: {
                connectedBattery->setState(BATTERY_TESTING);
                action = type;
                setStage(TEST_LOAD_CONFIG);
                cutoffVoltageCheckTimer = millis();
                wattHours = 0.;
                powerReadTimer = millis();
                eManager->conditionerEvent(conditionerID, connectedBattery->getID(), EVENT_CONDITIONER_START, CONDITIONER_CONNECTED, CONDITIONER_TEST_QUICK, 0l);
                return true;
                break;
            }
            case DISCHARGE_TEST_HIGH_RATE: {
                connectedBattery->setState(BATTERY_TESTING);
                action = type;
                cutoffVoltageCheckTimer = millis();
                wattHours = 0.;
                highRateTestResult = 0.;
                powerReadTimer = millis();
                this->waitForTest = waitForTest;
                if (!waitForTest) {
                    setStage(TEST_LOAD_CONFIG);
                    testWaitStartTime = millis();
                }
                else
                    state = CONDITIONER_TEST_HIGH_RATE;
                eManager->conditionerEvent(conditionerID, connectedBattery->getID(), EVENT_CONDITIONER_START, CONDITIONER_CONNECTED, CONDITIONER_TEST_HIGH_RATE, 0l);
                return true;
                break;
            }
        }
    }
    return false;
}

//returns the state of this conditioner
conditioner_state Conditioner::getState() {
    return state;
}

//controls the states of the loads and what happens for each
void Conditioner::setStage(load_config newStage) {
    //conditioner_state oldState = state;
    stage = newStage;
    setState(stage);
    setPins(stage);
}

void Conditioner::setState(load_config newStage) {
    if (action == DISCHARGE_TEST_HIGH_RATE && newStage == TEST_LOAD_CONFIG) {
        state = CONDITIONER_TEST_HIGH_RATE;
    }
    else if (action == DISCHARGE_TEST_QUICK && newStage == TEST_LOAD_CONFIG) {
        state = CONDITIONER_TEST_QUICK;
    }
    else {
        switch (newStage) {
            case CONDITIONER_LOAD_CONFIG_NO_LOAD: {
                state = CONDITIONER_IDLE;
                //connectedBattery->setState(BATTERY_CONNECTED_TO_CONDITIONER);
                connectedBattery->setState(BATTERY_IDLE);
                break;
            }

            case CONDITIONER_LOAD_CONFIG_300W: {
                state = CONDITIONER_CONDITION_300W;
                break;
            }

            case CONDITIONER_LOAD_CONFIG_200W: {
                state = CONDITIONER_CONDITION_200W;
                break;
            }

            case CONDITIONER_LOAD_CONFIG_120W: {
                state = CONDITIONER_CONDITION_120W;
                break;
            }

            case CONDITIONER_LOAD_CONFIG_100W: {
                state = CONDITIONER_CONDITION_100W;
                break;
            }

            case CONDITIONER_LOAD_CONFIG_20W: {
                state = CONDITIONER_CONDITION_20W;
                break;
            }
        }
    }
}

//this stops the conditioning process
bool Conditioner::stop(bool halting) {
    if (state != CONDITIONER_IDLE) {
        conditioner_state oldState = state;
        setStage(CONDITIONER_LOAD_CONFIG_NO_LOAD);
        if (halting)
            eManager->conditionerEvent(conditionerID, connectedBattery->getID(), EVENT_CONDITIONER_HALT, oldState, state, 0l);
        else
            eManager->conditionerEvent(conditionerID, connectedBattery->getID(), EVENT_CONDITIONER_END, oldState, state, 0l);
        return true;
    }
    return false;
}

//sets the pins for the loads based on a boolean array
void Conditioner::setPins(load_config config) {
    //depending on which conditioner this is set a set of pins
    if (config == CONDITIONER_LOAD_CONFIG_NO_LOAD) {
        switchInverter(false);
    }
    else {
        switchInverter(true);
    }

    digitalWrite(PINS_LOADS[conditionerID][PIN_TYPE_COND_20_WATT], conditioner_loads[config][0]);
    digitalWrite(PINS_LOADS[conditionerID][PIN_TYPE_COND_100_WATT], conditioner_loads[config][1]);
    digitalWrite(PINS_LOADS[conditionerID][PIN_TYPE_COND_200_WATT], conditioner_loads[config][2]);
}

//connects or disconnects a battery from this inverter
void Conditioner::switchInverter(bool newPos) {
        uint8_t battID = connectedBattery->getID();
        digitalWrite(PINS_INVERTER[battID], newPos);
}

//adds the watt-hours calculated to the total watt-hours
void Conditioner::addWattHours(uint32_t time, double voltage, double current) {
    //Time is in ms, so convert it to hours
    //We want to be careful not to overflow our doubles as they can only store up to ~32k
    wattHours += voltage * current * ((double)time) / 1000. / 60. / 60.;
}

//gets the most recent watt-hour value for this conditioner
//if this conditioner is still conditioning, this value will be constantly increasing
double Conditioner::getLastWattHours() {
    return wattHours;
}

//gets the ID of the battery connected to this conditioner
uint8_t Conditioner::getConnectedBattID() {
    return connectedBattery->getID();
}

//calculates a double value between 10 and 12 volts from the voltage sensor for this conditioner
double Conditioner::getVoltage() {
    uint16_t pinValue = Utils::filteredAnalogRead(PINS_MEASUREMENT[conditionerID][PIN_TYPE_MEASUREMENT_VOLTAGE], ANALOG_FILTER_TYPE);
    float vin = ((double)pinValue) / ANALOG_MAX_VALUE * ANALOG_MAX_VOLTAGE;
    return (vin / COND_VOLTAGE_GAIN) + COND_VOLTAGE_REF;
}

//returns the voltage value at the analog pin as an int from 0-1023 with no calculations
uint16_t Conditioner::getRawVoltage() {
    return Utils::filteredAnalogRead(PINS_MEASUREMENT[conditionerID][PIN_TYPE_MEASUREMENT_VOLTAGE], ANALOG_FILTER_TYPE);
}

//returns the calculated current from 0-50 amps
double Conditioner::getCurrent() {
    uint16_t pinValue = Utils::filteredAnalogRead(PINS_MEASUREMENT[conditionerID][PIN_TYPE_MEASUREMENT_CURRENT], ANALOG_FILTER_TYPE);
    float vin = (((double)pinValue) / ANALOG_MAX_VALUE) * ANALOG_MAX_VOLTAGE;
    return vin / COND_CURRENT_GAIN / COND_CURRENT_OPAMP_RANGE * COND_MAX_AMPS;
}

//returns the current value at the analog pin as an int from 0-1023 with no calculations
uint16_t Conditioner::getRawCurrent() {
    return Utils::filteredAnalogRead(PINS_MEASUREMENT[conditionerID][PIN_TYPE_MEASUREMENT_CURRENT], ANALOG_READ_FILTER_RMS);
}
